/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写
 ***************************************************************************/

#include "belttrackmanager.h"
#include "domparser.h"
#include "coordinatemanager.h"
#include "regdata.h"
#include "motionmessage.h"
#include "visiontrack.h"
#include "belttrack.h"
#include "robotcontrol.h"
#include "robotio.h"
#define IF_DEBUG 1

extern RobotControl* robCtl;
#define D_BELT_TRACK_DI_INDEX 10


BeltTrackManager::BeltTrackManager(QString configFileIn, CoordinateManager *coordinateManagerIn, RegData *regDataIn,
                                   MotionMessage *motionMessageIn, int robotIdIn, double deltaTimeIn,
                                   RobotIo *ioControllerIn, QObject *parent) : QObject(parent)
{
    robotId=robotIdIn;
    configFile=configFileIn;
    motionMessage=motionMessageIn;
    coordinateManager=coordinateManagerIn;
    regData=regDataIn;
    deltaTime=deltaTimeIn;
    ioController=ioControllerIn;
    isInitialOkStatus=false;
    isUseTrack=false;

    if(1==initialConfigFile(configFileIn))
    {
        if(true==isUseTrack)
        {
            isInitialOkStatus=true;
//            clientSocket = new QTcpSocket;
//            if(1==connectSensorServer(sensorServerIp,sensorServerPort))
//            {
//                isConnectedStatus=true;
//            }

            //初始化皮带
            for(int i=0;i<beltConfigList.size();i++)
            {
                BeltTrack *tmpBeltTrack=new BeltTrack(i,regData,beltConfigList[i],motionMessage,coordinateManagerIn,deltaTimeIn);
                beltTrackMap.insert(i,tmpBeltTrack);
            }

            for(int i=0;i<visionConfigList.size();i++)
            {
                initialVision(i);
            }

            timer=new QTimer();
            connect( timer, SIGNAL(timeout()), this, SLOT(timeOut_slot()) );
            timer->start( 30 );


        }
        else
        {
            isInitialOkStatus=true;
        }

    }
    else
    {

            addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "BeltTrackManager", 22100, robotId);

    }
}

int BeltTrackManager::setCurrentFrameCommand(int beltIndexIn, Frame frameIn)
{
    if(beltTrackMap.contains(beltIndexIn))
    {
        return beltTrackMap.value(beltIndexIn)->setCurrentFrameCommand(frameIn);
    }
    addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "BeltTrackManager", 22101, robotId);
    return -1;
}

int BeltTrackManager::addCeaseMotion(int beltIndexIn, Frame startFrameIn, Frame endFrameIn)
{
    if(beltTrackMap.contains(beltIndexIn))
    {
        return beltTrackMap.value(beltIndexIn)->addCeaseMotion(startFrameIn,endFrameIn);
    }
    addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "BeltTrackManager", 22101, robotId);
    return -1;
}

int BeltTrackManager::moveOneStep_FrameCommand(int beltIndexIn, Frame &frameOut,double &durationLeftOut)
{
    if(beltTrackMap.contains(beltIndexIn))
    {
        return beltTrackMap.value(beltIndexIn)->moveOneStep_FrameCommand(frameOut,durationLeftOut);
    }
    addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "BeltTrackManager", 22101, robotId);
    return -1;
}

int BeltTrackManager::setWork(bool isOnIn)
{
    VisionTrack* tmpVsionTrack;
    foreach (tmpVsionTrack, visionTrackMap)
    {
        tmpVsionTrack->setWork(isOnIn);
    }
}

int BeltTrackManager::getCurrentTrackIndex()
{
    return currentTrackIndex;
}

int BeltTrackManager::getTrackTransionDistance_start(int beltIndexIn, double &deltaX, double &deltaY)
{
    if(beltTrackMap.contains(beltIndexIn))
    {
        return beltTrackMap.value(beltIndexIn)->getTrackTransionDistance_start(deltaX,deltaY);
    }
    addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "BeltTrackManager", 22101, robotId);
    return -1;
}

int BeltTrackManager::getTrackTransionDistance_end(int beltIndexIn, double &deltaX, double &deltaY)
{
    if(beltTrackMap.contains(beltIndexIn))
    {
        return beltTrackMap.value(beltIndexIn)->getTrackTransionDistance_end(deltaX,deltaY);
    }
    addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "BeltTrackManager", 22101, robotId);
    return -1;
}

int BeltTrackManager::setDebugFlag(int flagIn)
{
    VisionTrack* tmpVsionTrack;
    foreach (tmpVsionTrack, visionTrackMap) {
        tmpVsionTrack->setDebugFlag(flagIn);
    }

    BeltTrack* tmpBeltTrack;
    foreach (tmpBeltTrack, beltTrackMap) {
        tmpBeltTrack->setDebugFlag(flagIn);
    }
}

int BeltTrackManager::getEncoderValue_belt(int beltIndexIn, int &encoderValueOut)
{
    std::vector<BeltTrackConfig> beltconfigListOut;
    getBeltTrackConfigList(beltconfigListOut);

    if(beltIndexIn>=0 && beltIndexIn<beltconfigListOut.size())
    {
        return getEncoderValue_belt(beltTrackMap.value(beltIndexIn),encoderValueOut);
    }

    qDebug()<<"error,getEncoderValue_belt 2";
    return -3;
}

int BeltTrackManager::getEncoderValue_vision(int visionIndexIn, int &encoderValueOut)
{
    std::vector<VisionTrackConfig> configListOut;
    getVisionTrackConfigList(configListOut);

    if(visionIndexIn>=0 && visionIndexIn<configListOut.size())
    {
        std::vector<BeltTrackConfig> beltconfigListOut;
        getBeltTrackConfigList(beltconfigListOut);
        int tmpIndex=configListOut[visionIndexIn].beltTrackIndex;
        if(tmpIndex>=0 && tmpIndex<beltconfigListOut.size())
        {
            return getEncoderValue_belt(tmpIndex,encoderValueOut);
        }
        else
        {
            qDebug()<<"error,getEncoderValue_vision 3";
            return -2;
        }
    }
    qDebug()<<"error,getEncoderValue_vision 4";
    return -3;
}

int BeltTrackManager::forceConnectCamera(int visionIndex)
{
    if(0>visionIndex || visionIndex>=visionConfigList.size())
    {
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "BeltTrackManager", 22132, robotId);
        return -1;
    }
    if(visionTrackMap.contains(visionIndex))
    {

    }
    else
    {

        //不能在这里创建视觉实例，否则接收以太网数据会被延迟。
    }
    return visionTrackMap.value(visionIndex)->forceConnectCamera();
}

int BeltTrackManager::forceTrigCamera(int visionIndexIn, bool isHighIn)
{
    qDebug()<<"BeltTrackManager::forceTrigCamera visionIndexIn"<<visionIndexIn;
    std::vector<VisionTrackConfig> configListOut;
    getVisionTrackConfigList(configListOut);

    if(visionIndexIn>=0 && visionIndexIn<configListOut.size())
    {
        std::vector<BeltTrackConfig> beltconfigListOut;
        getBeltTrackConfigList(beltconfigListOut);
        int tmpIndex=configListOut[visionIndexIn].beltTrackIndex;
        if(4==configListOut[visionIndexIn].cameraTrigMethod)
        {
            return trigVisionOnce(visionIndexIn);
        }
        if(tmpIndex>=0 && tmpIndex<beltconfigListOut.size())
        {
            int doIndex=beltconfigListOut[tmpIndex].doIndexOnDevice;
            //编码器设备或多合一io设备．
            if(0==beltconfigListOut[tmpIndex].encoderDeviceType)
            {
//                ioController->setDo(doIndex,!isHighIn);
//                usleep(20000);
                ioController->setDo(doIndex,isHighIn);
            }
            else
            {
//                robCtl->setEncoderReceiverOutput_bit(doIndex,!isHighIn);
//                usleep(20000);
                robCtl->setEncoderReceiverOutput_bit(doIndex,isHighIn);
            }
            return 1;

        }
        else
        {
            qDebug()<<"forceTrigCamera,error1.";
            return -2;
        }
    }
    qDebug()<<"forceTrigCamera,error2.";
    return -1;
}

int BeltTrackManager::cease()
{
    BeltTrack* tmpBeltTrack;
    foreach (tmpBeltTrack, beltTrackMap)
    {
        tmpBeltTrack->cease();
    }

    return 1;
}

int BeltTrackManager::reset()
{
    VisionTrack* tmpVsionTrack;
    foreach (tmpVsionTrack, visionTrackMap)
    {
        tmpVsionTrack->reset();
    }

    BeltTrack* tmpBeltTrack;
    foreach (tmpBeltTrack, beltTrackMap)
    {
        tmpBeltTrack->reset();
    }

    return 1;
}


bool BeltTrackManager::isInitialOk()
{
    return isInitialOkStatus;

}


bool BeltTrackManager::isUseThisModule()
{
    return isUseTrack;

}

int BeltTrackManager::setVisionTrigObjectLength(int visionIndex, double lengthIn)
{
    if(0<=visionIndex && visionIndex<visionConfigList.size())
    {
        visionConfigList[visionIndex].objectLength=lengthIn;
        if(visionTrackMap.contains(visionIndex))
        {
            visionTrackMap.value(visionIndex)->setVisionTrigObjectLength(lengthIn);
            writeConfigFile(configFile);
            return 1;
        }
    }
    qDebug()<<"error, modifyVisionTrackConfig failed,visionIndex"<<visionIndex;
    return 0;
}

int BeltTrackManager::setTrackOffset(int beltIndex,double offset_x, double offset_y)
{
    if(0>beltIndex || beltIndex>=beltConfigList.size())
    {
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "BeltTrackManager", 22101, robotId);
        return -1;
    }
    if(beltTrackMap.contains(beltIndex))
    {
        return beltTrackMap.value(beltIndex)->setTrackOffset(offset_x,offset_y);
    }
    return -2;
}

int BeltTrackManager::getVisionTrackConfigList(std::vector<VisionTrackConfig> &configListOut)
{
    configListOut=visionConfigList.toStdVector();
    return 1;
}

int BeltTrackManager::getBeltTrackConfigList(std::vector<BeltTrackConfig> &configListOut)
{
    configListOut=beltConfigList.toStdVector();
    return 1;
}

int BeltTrackManager::modifyBeltTrackConfig(int beltIndex, BeltTrackConfig configIn)
{
    if(0<=beltIndex && beltIndex<beltConfigList.size())
    {
        beltConfigList[beltIndex]=configIn;
        if(beltTrackMap.contains(beltIndex))
        {
            beltTrackMap.value(beltIndex)->modifyBeltTrackConfig(configIn);
            beltConfigList[beltIndex].beltTransimission=beltTrackMap.value(beltIndex)->getBeltTransmission();
            writeConfigFile(configFile);
            return 1;
        }

    }
    qDebug()<<"error, modifyBeltTrackConfig failed,beltIndex"<<beltIndex;
    return 0;
}

int BeltTrackManager::modifyVisionTrackConfig(int visionIndex, VisionTrackConfig configIn)
{
    if(0<=visionIndex && visionIndex<visionConfigList.size())
    {
        visionConfigList[visionIndex]=configIn;
        if(visionTrackMap.contains(visionIndex))
        {
            visionTrackMap.value(visionIndex)->modifyVisionTrackConfig(configIn);
            writeConfigFile(configFile);
            return 1;
        }
    }
    qDebug()<<"error, modifyVisionTrackConfig failed,visionIndex"<<visionIndex;
    return 0;
}

int BeltTrackManager::getVisionTrackDebugInfo(int visionIndex, VisionTrackDebugInfo &visionInfoOut)
{
    visionInfoOut.isConnectedStatus=false;
    if(0>visionIndex || visionIndex>=visionConfigList.size())
    {
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "BeltTrackManager", 22132, robotId);
        return -1;
    }
    if(visionTrackMap.contains(visionIndex))
    {
        return visionTrackMap.value(visionIndex)->getVisionTrackDebugInfo(visionInfoOut);
    }
    return -2;

}

int BeltTrackManager::getBeltTrackDebugInfo(int beltIndex, BeltTrackDebugInfo &beltTrackInfoOut)
{
    if(0>beltIndex || beltIndex>=beltConfigList.size())
    {
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "BeltTrackManager", 22101, robotId);
        return -1;
    }
    if(beltTrackMap.contains(beltIndex))
    {
        return beltTrackMap.value(beltIndex)->getBeltTrackDebugInfo(beltTrackInfoOut);
    }
    return -2;
}

int BeltTrackManager::runVision(int visionIndex)
{
    if(0>visionIndex || visionIndex>=visionConfigList.size())
    {
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "BeltTrackManager", 22132, robotId);
        return -1;
    }
    if(visionTrackMap.contains(visionIndex))
    {

    }
    else
    {

        //不能在这里创建视觉实例，否则接收以太网数据会被延迟。
    }
    return visionTrackMap.value(visionIndex)->runVision();
}



int BeltTrackManager::trigVisionOnce(int visionIndex)
{
    if(visionTrackMap.contains(visionIndex))
    {
        return visionTrackMap.value(visionIndex)->trigVisionOnce();
    }
    addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "BeltTrackManager", 22132, robotId);
    return -1;
}

int BeltTrackManager::getVisionData(int visionIndex)
{
    if(visionTrackMap.contains(visionIndex))
    {
        return visionTrackMap.value(visionIndex)->getVisionData();
    }
    addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "BeltTrackManager", 22132, robotId);
    return -1;
}

int BeltTrackManager::clearVisionData(int visionIndex)
{
    if(visionTrackMap.contains(visionIndex))
    {
        return visionTrackMap.value(visionIndex)->clearVisionData();
    }
    addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "BeltTrackManager", 22132, robotId);
    return -1;
}

int BeltTrackManager::loopCommontimeOnce()
{
    VisionTrack* tmpVsionTrack;
    foreach (tmpVsionTrack, visionTrackMap) {
        tmpVsionTrack->loopCommontimeOnce();
    }

    BeltTrack* tmpBeltTrack;
    foreach (tmpBeltTrack, beltTrackMap) {
        tmpBeltTrack->loopCommontimeOnce();
    }
}

int BeltTrackManager::loopRealtimeOnce(QVector<PointPro> &deltaPoseOut,
                                       QVector<E_MOVE_STAGE> &isInHoldingStatusOut)
{

    VisionTrack* tmpVsionTrack;
    foreach (tmpVsionTrack, visionTrackMap)
    {
        bool detectSi=ioController->getSi(D_BELT_TRACK_DI_INDEX);//增加相机精度用的di传感器信号
        bool isOutPutChange;
        bool doValue;
        tmpVsionTrack->loopRealtimeOnce(detectSi,doValue,isOutPutChange);
        if(isOutPutChange)
        {
            int doDeviceTypeOut,  doIndexOut;
            tmpVsionTrack->getDoType(doDeviceTypeOut,  doIndexOut);
            if(0==doDeviceTypeOut)
            {
                ioController->setDo(doIndexOut,doValue);
            }
            else
            {
                robCtl->setEncoderReceiverOutput_bit(doIndexOut,doValue);
            }
        }
    }

    deltaPoseOut.clear();
    isInHoldingStatusOut.clear();
    BeltTrack* tmpBeltTrack;
    foreach (tmpBeltTrack, beltTrackMap)
    {
        int encoderData;
        getEncoderValue_belt(tmpBeltTrack,encoderData);
        PointPro tmpdeltaPoseOut;
        E_MOVE_STAGE tmpstageOut;
        tmpBeltTrack->loopRealtimeOnce(encoderData,tmpdeltaPoseOut,tmpstageOut);
        deltaPoseOut.append(tmpdeltaPoseOut);
        isInHoldingStatusOut.append(tmpstageOut);
    }

    return 1;
}

int BeltTrackManager::getEncoderValue_belt(BeltTrack *tmpBeltTrackIn, int& encoderDataOut)
{
    int deviceTypeOut, aliasOut,encoderIndexOut;
    tmpBeltTrackIn->getEncoderDeviceType(deviceTypeOut, aliasOut,encoderIndexOut);
    if(0==deviceTypeOut)
    {
        EncoderReceiverData tmpencoderData;
        if(1!=robCtl->getEncoderReceiverData_iomodule(aliasOut, tmpencoderData))
        {
//            qDebug()<<"error,1!=robCtl->getEncoderReceiverData_iomodule(aliasOut, tmpencoderData)";
            return -4;
        }
        if(0==encoderIndexOut)
        {
            encoderDataOut=tmpencoderData.Encoder0_Val;
        }
        else if(1==encoderIndexOut)
        {
            encoderDataOut=tmpencoderData.Encoder1_Val;
        }
        else if(2==encoderIndexOut)
        {
            encoderDataOut=tmpencoderData.Encoder2_Val;
        }
        else
        {
            qDebug()<<"error,encoderData index error ,,, of iomodule";
            return -2;
        }
    }
    else
    {
        EncoderReceiverData tmpencoderData;
        if(1!=robCtl->getEncoderReceiverData( tmpencoderData))
        {
//            qDebug()<<"error,1!=robCtl->getEncoderReceiverData( tmpencoderData)";
            return -5;
        }
        if(0==encoderIndexOut)
        {
            encoderDataOut=tmpencoderData.Encoder0_Val;
        }
        else if(1==encoderIndexOut)
        {
            encoderDataOut=tmpencoderData.Encoder1_Val;
        }
        else if(2==encoderIndexOut)
        {
            encoderDataOut=tmpencoderData.Encoder2_Val;
        }
        else
        {
            qDebug()<<"error,encoderData index error 2aa";
            return -2;
        }

    }
    return 1;
}

int BeltTrackManager::initialVision(int visionIndex)
{
    if(visionConfigList[visionIndex].isTrack)
    {
        if(0>visionConfigList[visionIndex].beltTrackIndex || visionConfigList[visionIndex].beltTrackIndex>=beltConfigList.size())
        {
            addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "BeltTrackManager", 22132, robotId);
            return -1;
        }
//            BeltTrack *tmpBeltTrack=new BeltTrack(beltConfigList[visionConfigList[visionIndex].beltTrackIndex],motionMessage);
//            beltTrackMap.insert(visionConfigList[visionIndex].beltTrackIndex,tmpBeltTrack);
        VisionTrack* tmpVisionTrack=new VisionTrack(visionIndex,regData,visionConfigList[visionIndex],motionMessage,deltaTime,
                                                    beltTrackMap.value(visionConfigList[visionIndex].beltTrackIndex));
        visionTrackMap.insert(visionIndex,tmpVisionTrack);
        if(beltTrackMap.contains(visionConfigList[visionIndex].beltTrackIndex))
        {
            beltTrackMap.value(visionConfigList[visionIndex].beltTrackIndex)->
                    setOriginalToolCoordinateIndex(visionConfigList[visionIndex].userCoordinateIndex);
        }
        else
        {
            addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "BeltTrackManager", 22104, robotId,visionConfigList[visionIndex].beltTrackIndex);
            return -1;
        }

    }
    else
    {
         VisionTrack* tmpVisionTrack=new VisionTrack(visionIndex,regData,visionConfigList[visionIndex],motionMessage,deltaTime);
         visionTrackMap.insert(visionIndex,tmpVisionTrack);
    }

    return 1;


}

int BeltTrackManager::getTrackData(int beltIndex)
{
    if(beltTrackMap.contains(beltIndex))
    {
        return beltTrackMap.value(beltIndex)->getTrackData();
    }
    addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "BeltTrackManager", 22101, robotId);
    return -1;
}

int BeltTrackManager::trackStart(int beltIndex)
{
    if(beltTrackMap.contains(beltIndex))
    {
        currentTrackIndex=beltIndex;
        return beltTrackMap.value(beltIndex)->trackStart();
    }
    addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "BeltTrackManager", 22101, robotId);
    return -1;
}

int BeltTrackManager::trackEnd(int beltIndex)
{
    if(beltTrackMap.contains(beltIndex))
    {
        return beltTrackMap.value(beltIndex)->trackEnd();
    }
    addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "BeltTrackManager", 22101, robotId);
    return -1;
}

#define D_PARAMETER_BELT_NODE_COUNT 27
#define D_PARAMETER_VISION_NODE_COUNT 20
int BeltTrackManager::initialConfigFile(QString configFileName)
{
    qDebug() << "BeltTrackManager::initialConfigFile ==== 初始化配置文件：" << configFileName;

    beltConfigList.clear();
    visionConfigList.clear();

    DomParser domparser;
    bool ok = domparser.openXml(configFileName, QIODevice::ReadOnly);
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << " 配置文件打开失败：" << configFileName;
        }
        return -1;
    }

    QDomNode domNode = domparser.findSubNode(domparser.getRootDomElement(), "isUseTrack", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "initialConfigFile failed" << configFileName;
        }
            return -1;
    }
    domparser.readXml(domNode, isUseTrack);
    if(false==isUseTrack)
    {
        qDebug() << "false==isUseTrack" << configFileName;
        return 1;
    }

    ///beltParameter读参数
    QDomNode  inNode = domparser.findSubNode(domparser.getRootDomElement(), "beltParameter", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug()<<"读文件失败: "<<configFileName;
        }
        return -1;
    }

    int count;

   domparser. readXml( domparser.findSubNode(inNode, "count", ok), count  );
    if(!ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"读文件失败: ";
        }
        return -1;
    }


    QVector<bool> okReturn;
    okReturn.resize(D_PARAMETER_BELT_NODE_COUNT);
    for(int i=0;i<count;i++)
    {
         QDomNode  tmpNode =  domparser.findSubNode(inNode, QString("belt%1").arg(i), ok);
         if( !ok  )
         {
            if(IF_DEBUG)
            {
                qDebug()<<"读内部参数失败:  "<<i;
            }
            return -1;
         }
        BeltTrackConfig tmpConfig;
        //节点读取
        domparser.readXml( domparser.findSubNode(tmpNode, "trackFunction", okReturn[0]),  tmpConfig.trackFunction  );
        domparser.readXml( domparser.findSubNode(tmpNode, "encoderDeviceAlias", okReturn[1]),  tmpConfig.encoderDeviceAlias );
        domparser.readXml( domparser.findSubNode(tmpNode, "encoderIndexOnDevice", okReturn[2]),  tmpConfig.encoderIndexOnDevice  );
        domparser.readXml( domparser.findSubNode(tmpNode, "trackTransition_startDistance", okReturn[3]),  tmpConfig.trackTransition_startDistance  );
        domparser.readXml( domparser.findSubNode(tmpNode, "validTrackStartPoint_fromA", okReturn[4]),  tmpConfig.validTrackStartPoint_fromA  );
        domparser.readXml( domparser.findSubNode(tmpNode, "validTrackEndPoint_fromA", okReturn[5]),  tmpConfig.validTrackEndPoint_fromA  );
        domparser.readXml( domparser.findSubNode(tmpNode, "encoderValue_referenceAPoint", okReturn[6]),  tmpConfig.encoderValue_referenceAPoint  );
        domparser.readXml( domparser.findSubNode(tmpNode, "encoderValue_referenceBPoint", okReturn[7]),  tmpConfig.encoderValue_referenceBPoint  );
        domparser.readXml( domparser.findSubNode(tmpNode, "trackPointOffset_x", okReturn[8]),  tmpConfig.trackPointOffset_x  );
        domparser.readXml( domparser.findSubNode(tmpNode, "trackPointOffset_y", okReturn[9]),  tmpConfig.trackPointOffset_y  );
        domparser.readXml( domparser.findSubNode(tmpNode, "trackPointOffset_z", okReturn[10]),  tmpConfig.trackPointOffset_z  );
        domparser.readXml( domparser.findSubNode(tmpNode, "objectMininumDistance", okReturn[11]),  tmpConfig.objectMininumDistance  );
        domparser.readXml( domparser.findSubNode(tmpNode, "startSprayPoint_fromA", okReturn[12]),  tmpConfig.startSprayPoint_fromA  );
        domparser.readXml( domparser.findSubNode(tmpNode, "workpieceDistance", okReturn[13]),  tmpConfig.workpieceDistance  );
        domparser.readXml( domparser.findSubNode(tmpNode, "stopDistance", okReturn[14]),  tmpConfig.stopDistance  );
        domparser.readXml( domparser.findSubNode(tmpNode, "beltVelocity", okReturn[15]),  tmpConfig.beltVelocity  );
        domparser.readXml( domparser.findSubNode(tmpNode, "trackSpeedAdjust", okReturn[16]),  tmpConfig.trackSpeedAdjust  );
        domparser.readXml( domparser.findSubNode(tmpNode, "beltTransimissionAdjust", okReturn[17]),  tmpConfig.beltTransimissionAdjust  );
        domparser.readXml( domparser.findSubNode(tmpNode, "encoderDirection", okReturn[18]),  tmpConfig.encoderDirection  );
        domparser.readXml( domparser.findSubNode(tmpNode, "xValue_referenceAPoint", okReturn[19]),  tmpConfig.xValue_referenceAPoint  );
        domparser.readXml( domparser.findSubNode(tmpNode, "xValue_referenceBPoint", okReturn[20]),  tmpConfig.xValue_referenceBPoint  );
        domparser.readXml( domparser.findSubNode(tmpNode, "doIndexOnDevice", okReturn[21]),  tmpConfig.doIndexOnDevice  );
        domparser.readXml( domparser.findSubNode(tmpNode, "encoderDeviceType", okReturn[22]),  tmpConfig.encoderDeviceType  );
        domparser.readXml( domparser.findSubNode(tmpNode, "yValue_referenceAPoint", okReturn[23]),  tmpConfig.yValue_referenceAPoint  );
        domparser.readXml( domparser.findSubNode(tmpNode, "yValue_referenceBPoint", okReturn[24]),  tmpConfig.yValue_referenceBPoint  );
        domparser.readXml( domparser.findSubNode(tmpNode, "trackTransition_endDistance", okReturn[25]),  tmpConfig.trackTransition_endDistance  );
        domparser.readXml( domparser.findSubNode(tmpNode, "beltTransimission", okReturn[D_PARAMETER_BELT_NODE_COUNT-1]),  tmpConfig.beltTransimission  );

        for(int k=0;k<okReturn.size();k++)
        {
           if( !okReturn[k]  )
           {
              if(IF_DEBUG)
              {
                  qDebug()<<"ZeroManagement::readFile()====读内部轴参数节点失败: axis "<<i<<"axisParameter"<<k;
              }
              return -1;
           }
        }
        beltConfigList.append(tmpConfig);
    }

    ///visionParameter读参数
    inNode = domparser.findSubNode(domparser.getRootDomElement(), "visionParameter", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug()<<"读文件失败: "<<configFileName;
        }
        return -1;
    }


   domparser. readXml( domparser.findSubNode(inNode, "count", ok), count  );
    if(!ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"读文件失败: ";
        }
        return -1;
    }

    okReturn.resize(D_PARAMETER_BELT_NODE_COUNT);
    for(int i=0;i<count;i++)
    {
         QDomNode  tmpNode =  domparser.findSubNode(inNode, QString("vision%1").arg(i), ok);
         if( !ok  )
         {
            if(IF_DEBUG)
            {
                qDebug()<<"读内部参数失败:  "<<i;
            }
            return -1;
         }
        VisionTrackConfig tmpConfig;
        //节点读取
        domparser.readXml( domparser.findSubNode(tmpNode, "userCoordinateIndex", okReturn[0]),  tmpConfig.userCoordinateIndex  );
        QString tmpIp;
        domparser.readXml( domparser.findSubNode(tmpNode, "cameraIp", okReturn[1]),  tmpIp );
        tmpConfig.cameraIp=tmpIp.toStdString();
        domparser.readXml( domparser.findSubNode(tmpNode, "cameraPort", okReturn[2]),  tmpConfig.cameraPort  );
        domparser.readXml( domparser.findSubNode(tmpNode, "cameraProtocol", okReturn[3]),  tmpConfig.cameraProtocol  );
        domparser.readXml( domparser.findSubNode(tmpNode, "cameraCommunicateTimeout", okReturn[4]),  tmpConfig.cameraCommunicateTimeout  );
        domparser.readXml( domparser.findSubNode(tmpNode, "cameraMmPerPix_x", okReturn[5]),  tmpConfig.cameraMmPerPix_x  );
        domparser.readXml( domparser.findSubNode(tmpNode, "cameraMmPerPix_y", okReturn[6]),  tmpConfig.cameraMmPerPix_y  );
        domparser.readXml( domparser.findSubNode(tmpNode, "isTrack", okReturn[7]),  tmpConfig.isTrack  );
        domparser.readXml( domparser.findSubNode(tmpNode, "beltTrackIndex", okReturn[8]),  tmpConfig.beltTrackIndex  );
        domparser.readXml( domparser.findSubNode(tmpNode, "cameraTrigMethod", okReturn[9]),  tmpConfig.cameraTrigMethod  );
        domparser.readXml( domparser.findSubNode(tmpNode, "trigTimeOrDistance", okReturn[10]),  tmpConfig.trigTimeOrDistance  );
        domparser.readXml( domparser.findSubNode(tmpNode, "objectMininumDistance", okReturn[11]),  tmpConfig.objectMininumDistance  );
        domparser.readXml( domparser.findSubNode(tmpNode, "isWorkZoneCoincident", okReturn[12]),  tmpConfig.isWorkZoneCoincident  );
        domparser.readXml( domparser.findSubNode(tmpNode, "visionZoneEncoderRecorded", okReturn[13]),  tmpConfig.visionZoneEncoderRecorded  );
        domparser.readXml( domparser.findSubNode(tmpNode, "cameraOffset_x", okReturn[14]),  tmpConfig.cameraOffset_x  );
        domparser.readXml( domparser.findSubNode(tmpNode, "cameraOffset_y", okReturn[15]),  tmpConfig.cameraOffset_y  );
        domparser.readXml( domparser.findSubNode(tmpNode, "cameraOffset_r", okReturn[16]),  tmpConfig.cameraOffset_r  );
        domparser.readXml( domparser.findSubNode(tmpNode, "objectLength", okReturn[17]),  tmpConfig.objectLength  );
        domparser.readXml( domparser.findSubNode(tmpNode, "trigFilterRatio", okReturn[18]),  tmpConfig.trigFilterRatio  );
        domparser.readXml( domparser.findSubNode(tmpNode, "robotZoneEncoderRecorded", okReturn[D_PARAMETER_VISION_NODE_COUNT-1]),  tmpConfig.robotZoneEncoderRecorded  );

        for(int k=0;k<okReturn.size();k++)
        {
           if( !okReturn[k]  )
           {
              if(IF_DEBUG)
              {
                  qDebug()<<"ZeroManagement::readFile()====读内部轴参数节点失败: axis "<<i<<"axisParameter"<<k;
              }
              return -1;
           }
        }
        visionConfigList.append(tmpConfig);
    }

    domparser.closeXml();
    return 1;
}

int BeltTrackManager::writeConfigFile(QString configFileName)
{
    qDebug() << "BeltTrackManager::writeConfigFile ==== 初始化配置文件：" << configFileName;


    DomParser domparser;
    bool ok = domparser.openXml(configFileName, QIODevice::ReadWrite);
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << " 配置文件打开失败：" << configFileName;
        }
        return -1;
    }

    QDomNode domNode = domparser.findSubNode(domparser.getRootDomElement(), "isUseTrack", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "initialConfigFile failed" << configFileName;
        }
        return -1;
    }
    domparser.readXml(domNode, isUseTrack);

    ///beltParameter读参数
    QDomNode  inNode = domparser.findSubNode(domparser.getRootDomElement(), "beltParameter", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug()<<"读文件失败: "<<configFileName;
        }
        return -1;
    }

    int count;

   domparser. readXml( domparser.findSubNode(inNode, "count", ok), count  );
    if(!ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"读文件失败: ";
        }
        return -1;
    }


    QVector<bool> okReturn;
    okReturn.resize(D_PARAMETER_BELT_NODE_COUNT);
    for(int i=0;i<count;i++)
    {
         QDomNode  tmpNode =  domparser.findSubNode(inNode, QString("belt%1").arg(i), ok);
         if( !ok  )
         {
            if(IF_DEBUG)
            {
                qDebug()<<"读内部参数失败:  "<<i;
            }
            return -1;
         }
        BeltTrackConfig tmpConfig=beltConfigList[i];
        //节点写
        domparser.writeXml( domparser.findSubNode(tmpNode, "trackFunction", okReturn[0]),  tmpConfig.trackFunction  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "encoderDeviceAlias", okReturn[1]),  tmpConfig.encoderDeviceAlias );
        domparser.writeXml( domparser.findSubNode(tmpNode, "encoderIndexOnDevice", okReturn[2]),  tmpConfig.encoderIndexOnDevice  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "trackTransition_startDistance", okReturn[3]),  tmpConfig.trackTransition_startDistance  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "validTrackStartPoint_fromA", okReturn[4]),  tmpConfig.validTrackStartPoint_fromA  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "validTrackEndPoint_fromA", okReturn[5]),  tmpConfig.validTrackEndPoint_fromA  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "encoderValue_referenceAPoint", okReturn[6]),  tmpConfig.encoderValue_referenceAPoint  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "encoderValue_referenceBPoint", okReturn[7]),  tmpConfig.encoderValue_referenceBPoint  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "trackPointOffset_x", okReturn[8]),  tmpConfig.trackPointOffset_x  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "trackPointOffset_y", okReturn[9]),  tmpConfig.trackPointOffset_y  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "trackPointOffset_z", okReturn[10]),  tmpConfig.trackPointOffset_z  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "objectMininumDistance", okReturn[11]),  tmpConfig.objectMininumDistance  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "startSprayPoint_fromA", okReturn[12]),  tmpConfig.startSprayPoint_fromA  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "workpieceDistance", okReturn[13]),  tmpConfig.workpieceDistance  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "stopDistance", okReturn[14]),  tmpConfig.stopDistance  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "beltVelocity", okReturn[15]),  tmpConfig.beltVelocity  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "trackSpeedAdjust", okReturn[16]),  tmpConfig.trackSpeedAdjust  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "beltTransimissionAdjust", okReturn[17]),  tmpConfig.beltTransimissionAdjust  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "encoderDirection", okReturn[18]),  tmpConfig.encoderDirection  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "xValue_referenceAPoint", okReturn[19]),  tmpConfig.xValue_referenceAPoint  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "xValue_referenceBPoint", okReturn[20]),  tmpConfig.xValue_referenceBPoint  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "doIndexOnDevice", okReturn[21]),  tmpConfig.doIndexOnDevice  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "encoderDeviceType", okReturn[22]),  tmpConfig.encoderDeviceType  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "yValue_referenceAPoint", okReturn[23]),  tmpConfig.yValue_referenceAPoint  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "yValue_referenceBPoint", okReturn[24]),  tmpConfig.yValue_referenceBPoint  );
domparser.writeXml( domparser.findSubNode(tmpNode, "trackTransition_endDistance", okReturn[25]),  tmpConfig.trackTransition_endDistance  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "beltTransimission", okReturn[D_PARAMETER_BELT_NODE_COUNT-1]),  tmpConfig.beltTransimission  );

        for(int k=0;k<okReturn.size();k++)
        {
           if( !okReturn[k]  )
           {
              if(IF_DEBUG)
              {
                  qDebug()<<"BeltTrackManager内部轴参数节点失败: axis "<<i<<"axisParameter"<<k;
              }
              return -1;
           }
        }

    }

    ///visionParameter读参数
    inNode = domparser.findSubNode(domparser.getRootDomElement(), "visionParameter", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug()<<"读文件失败: "<<configFileName;
        }
        return -1;
    }


   domparser. readXml( domparser.findSubNode(inNode, "count", ok), count  );
    if(!ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"读文件失败: ";
        }
        return -1;
    }

    okReturn.resize(D_PARAMETER_BELT_NODE_COUNT);
    for(int i=0;i<count;i++)
    {
         QDomNode  tmpNode =  domparser.findSubNode(inNode, QString("vision%1").arg(i), ok);
         if( !ok  )
         {
            if(IF_DEBUG)
            {
                qDebug()<<"读内部参数失败:  "<<i;
            }
            return -1;
         }
        VisionTrackConfig tmpConfig=visionConfigList[i];
        //节点读取
        domparser.writeXml( domparser.findSubNode(tmpNode, "userCoordinateIndex", okReturn[0]),  tmpConfig.userCoordinateIndex  );
        QString tmpIp=QString::fromStdString(tmpConfig.cameraIp);
        domparser.writeXml( domparser.findSubNode(tmpNode, "cameraIp", okReturn[1]),  tmpIp );
        domparser.writeXml( domparser.findSubNode(tmpNode, "cameraPort", okReturn[2]),  tmpConfig.cameraPort  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "cameraProtocol", okReturn[3]),  tmpConfig.cameraProtocol  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "cameraCommunicateTimeout", okReturn[4]),  tmpConfig.cameraCommunicateTimeout  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "cameraMmPerPix_x", okReturn[5]),  tmpConfig.cameraMmPerPix_x  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "cameraMmPerPix_y", okReturn[6]),  tmpConfig.cameraMmPerPix_y  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "isTrack", okReturn[7]),  tmpConfig.isTrack  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "beltTrackIndex", okReturn[8]),  tmpConfig.beltTrackIndex  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "cameraTrigMethod", okReturn[9]),  tmpConfig.cameraTrigMethod  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "trigTimeOrDistance", okReturn[10]),  tmpConfig.trigTimeOrDistance  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "objectMininumDistance", okReturn[11]),  tmpConfig.objectMininumDistance  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "isWorkZoneCoincident", okReturn[12]),  tmpConfig.isWorkZoneCoincident  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "visionZoneEncoderRecorded", okReturn[13]),  tmpConfig.visionZoneEncoderRecorded  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "cameraOffset_x", okReturn[14]),  tmpConfig.cameraOffset_x  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "cameraOffset_y", okReturn[15]),  tmpConfig.cameraOffset_y  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "cameraOffset_r", okReturn[16]),  tmpConfig.cameraOffset_r  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "objectLength", okReturn[17]),  tmpConfig.objectLength  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "trigFilterRatio", okReturn[18]),  tmpConfig.trigFilterRatio  );
        domparser.writeXml( domparser.findSubNode(tmpNode, "robotZoneEncoderRecorded", okReturn[D_PARAMETER_VISION_NODE_COUNT-1]),  tmpConfig.robotZoneEncoderRecorded  );

        for(int k=0;k<okReturn.size();k++)
        {
           if( !okReturn[k]  )
           {
              if(IF_DEBUG)
              {
                  qDebug()<<"BeltTrackManager读内部轴参数节点失败: axis "<<i<<"axisParameter"<<k;
              }
              return -1;
           }
        }

    }

    domparser.closeXml(true);
    return 1;
}

void BeltTrackManager::addMsg(int messageLevel, std::string componentName, std::string messageType
                            , int messageCode, int robotIdIn, int parameter1, int parameter2)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageCode = messageCode;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotIdIn;

    string infomationStr;
    QString tmpStr;

    switch(messageCode)
    {
    case 22132:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(")视觉工艺号(%1)超出范围！").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 22131:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(")编码器序号(%1)超过支持区间！").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 22104:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(")跟踪工艺号%1不存在！").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 22103:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(")发送相机拍照命令失败！");
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 22102:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(")皮带使用的跟踪相机套接字创建失败！");
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 22101:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(")皮带号(%1)超出范围！").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 22100:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",跟踪工艺文件读取失败");
        infomationStr = tmpStr.toStdString();
        break;
    }



        default:
        {
            qDebug()<<"BeltTrackManager::addMsg====not match error code";
            break;
        }
    }

    tmpMsg.MessageInformation = infomationStr;

    #ifndef MOTION_SERVER_UNIT_TEST
    motionMessage->addMsg(tmpMsg);
#endif
}

void BeltTrackManager::timeOut_slot()
{
    loopCommontimeOnce();
}

